#include "FrameWork.h"

using namespace qglviewer;
using namespace std;

void Macro::reset()
{
	Macro::s_record = false;
	Macro::s_mouseControl = false;
	Macro::s_replay = false;

	Macro::s_timeStep = 0.0;

	Macro::s_replayIters = 5;
	Macro::s_mu1 = 14.32;
	Macro::s_mu2 = 4.2;

	Macro::s_gain = 5.0;
	Macro::s_maxForce = 500.0;
}

FrameWork::FrameWork()
{
	initData();

	createMenus();
	createParamGroup();
	createNoiseGroup();
	createGroupLayout();

	mainLayout = new QHBoxLayout;

	mainLayout->setMenuBar(mBar);
	
	rd->setMinimumSize(1600, 900);
	mainLayout->addWidget(rd);

	mainLayout->addLayout(groupLayout);

	setLayout(mainLayout);
	setWindowTitle("Nonprehensile Manipulation");
}

void FrameWork::initData()
{
	Macro::reset();
	pw = new PhysicsWorld;
	model = new Model(pw);
	model->loadModel("data/ball.xml");
	traj = new Trajectory();
	pw->traj = traj;
	ccd = new IKSolver(pw);
	rd = new Render(pw, ccd, model);
}

void FrameWork::createMenus()
{
	mBar = new QMenuBar;

	//file menu	
	fileMenu = new QMenu(tr("&File"), this);
	openFileAct = new QAction(tr("&Open..."), this);
	fileMenu->addAction(openFileAct);
	connect(openFileAct, SIGNAL(triggered()), this, SLOT(openFile()));

	saveFileAct = new QAction(tr("&Save As..."), this);
	fileMenu->addAction(saveFileAct);
	connect(saveFileAct, SIGNAL(triggered()), this, SLOT(saveFile()));

	fileMenu->addSeparator();

	exitFileAct = new QAction(tr("E&xit"), this);
	fileMenu->addAction(exitFileAct);
	connect(exitFileAct, SIGNAL(triggered()), this, SLOT(close()));

	//path menu
	pathMenu = new QMenu(tr("&Path"), this);

	loadPathAct = new QAction(tr("&Load..."), this);
	pathMenu->addAction(loadPathAct);
	connect(loadPathAct, SIGNAL(triggered()), this, SLOT(loadPath()));

	pathMenu->addSeparator();

	recordPathAct = new QAction(tr("&Record"), this);
	pathMenu->addAction(recordPathAct);
	connect(recordPathAct, SIGNAL(triggered()), this, SLOT(recordPath()));

	savePathAct = new QAction(tr("&Save As..."), this);
	pathMenu->addAction(savePathAct);
	connect(savePathAct, SIGNAL(triggered()), this, SLOT(savePath()));

	pathMenu->addSeparator();

	pausePathAct = new QAction(tr("&Pause"), this);
	pathMenu->addAction(pausePathAct);
	connect(pausePathAct, SIGNAL(triggered()), this, SLOT(pausePath()));

	//view menu
	viewMenu = new QMenu(tr("&View"), this);
	
	resetAllAct = new QAction(tr("&reset all"), this);
	viewMenu->addAction(resetAllAct);
	connect(resetAllAct, SIGNAL(triggered()), this, SLOT(resetAll()));

	resetDataAct = new QAction(tr("&reset data"), this);
	viewMenu->addAction(resetDataAct);
	connect(resetDataAct, SIGNAL(triggered()), this, SLOT(resetData()));

	resetParamAct = new QAction(tr("&reset params"), this);
	viewMenu->addAction(resetParamAct);
	connect(resetParamAct, SIGNAL(triggered()), this, SLOT(resetParam()));

	viewMenu->addSeparator();

	replayAct = new QAction(tr("&replay"), this);
	viewMenu->addAction(replayAct);
	connect(replayAct, SIGNAL(triggered()), this, SLOT(replay()));

	//add to menu bar
	mBar->addMenu(fileMenu);
	mBar->addSeparator();
	mBar->addMenu(pathMenu);
	mBar->addSeparator();
	mBar->addMenu(viewMenu);
}

void FrameWork::createParamGroup()
{
	paramGroup = new QGroupBox(tr("Parameters"));

	QGridLayout* layout = new QGridLayout;

	replayIterLabel = new QLabel(tr("Replay Iters (1 ~ 100): "));
	replayIterEditor = new QLineEdit(tr("5"), this);
	replayIterEditor->setValidator(new QIntValidator(1, 100, this));

	layout->addWidget(replayIterLabel, 1, 1);
	layout->addWidget(replayIterEditor, 1, 2);
	layout->setAlignment(Qt::AlignTop | Qt::AlignVCenter);

	connect(replayIterEditor, SIGNAL(textChanged(const QString &)), this, SLOT(changeReplayIters(const QString &)));

	paramGroup->setLayout(layout);
}

void FrameWork::createNoiseGroup()
{
	noiseGroup = new QGroupBox(tr("Noises"));

	QGridLayout* layout = new QGridLayout;

	muLabel1 = new QLabel(tr("mu1 (0 ~ 20): "));

	muSpinBox1 = new QDoubleSpinBox;
	muSpinBox1->setRange(0.0, 20.0);
	muSpinBox1->setSingleStep(0.2);
	muSpinBox1->setValue(Macro::s_mu1);

	muSlider1 = new QSlider(Qt::Horizontal);
	muSlider1->setRange(0, 1000);
	muSlider1->setSingleStep(10);
	muSlider1->setFocusPolicy(Qt::StrongFocus);
	muSlider1->setTickPosition(QSlider::TicksBothSides);
	muSlider1->setTickInterval(100);
	muSlider1->setValue((int)(Macro::s_mu1 / 20 * 1000));

	muLabel2 = new QLabel(tr("mu2 (0 ~ 10): "));

	muSpinBox2 = new QDoubleSpinBox;
	muSpinBox2->setRange(0.0, 10.0);
	muSpinBox2->setSingleStep(0.1);
	muSpinBox2->setValue(Macro::s_mu2);

	muSlider2 = new QSlider(Qt::Horizontal);
	muSlider2->setRange(0, 500);
	muSlider2->setSingleStep(5);
	muSlider2->setFocusPolicy(Qt::StrongFocus);
	muSlider2->setTickPosition(QSlider::TicksBothSides);
	muSlider2->setTickInterval(50);
	muSlider2->setValue((int)(Macro::s_mu2 / 10 * 500));

	massLabel = new QLabel(tr("box mass: "));
	massSpinBox = new QDoubleSpinBox;
	dMass m;
	dBodyGetMass(pw->magicBoxBody, &m);
	massSpinBox->setRange(0.0, 2.0);
	massSpinBox->setSingleStep(0.1);
	massSpinBox->setValue(m.mass);

	connect(muSlider1, SIGNAL(valueChanged(int)), this, SLOT(changeValueMuSpinBox1(int)));
	connect(muSlider2, SIGNAL(valueChanged(int)), this, SLOT(changeValueMuSpinBox2(int)));
	connect(muSpinBox1, SIGNAL(valueChanged(double)), this, SLOT(changeValueMuSlider1(double)));
	connect(muSpinBox2, SIGNAL(valueChanged(double)), this, SLOT(changeValueMuSlider2(double)));
	connect(massSpinBox, SIGNAL(valueChanged(double)), this, SLOT(changeValueMassSpinBox(double)));

	layout->addWidget(muLabel1, 1, 1);
	layout->addWidget(muSpinBox1, 1, 2);
	layout->addWidget(muSlider1, 1, 3);

	layout->addWidget(muLabel2, 2, 1);
	layout->addWidget(muSpinBox2, 2, 2);
	layout->addWidget(muSlider2, 2, 3);

	layout->addWidget(massLabel, 3, 1);
	layout->addWidget(massSpinBox, 3, 2);

	layout->setAlignment(Qt::AlignTop | Qt::AlignVCenter);

	noiseGroup->setLayout(layout);
}

void FrameWork::createGroupLayout()
{
	groupLayout = new QVBoxLayout;

	paramGroup->setMinimumSize(250, 400);
	groupLayout->addWidget(paramGroup);

	noiseGroup->setMinimumSize(250, 400);
	groupLayout->addWidget(noiseGroup);
}

void FrameWork::openFile()
{
	QString fileName = QFileDialog::getOpenFileName(this);
	//if (!fileName.isEmpty()) {
	//	addressWidget->readFromFile(fileName);
	//}
}

void FrameWork::saveFile()
{
	QString fileName = QFileDialog::getSaveFileName(this);
	//if (!fileName.isEmpty()) {
	//	addressWidget->writeToFile(fileName);
	//}
}

void FrameWork::recordPath()
{
	Macro::s_record = true;
	Macro::s_timeStep = 0.0;
	traj->record();
}

void FrameWork::savePath()
{
	QDateTime dt = QDateTime::currentDateTime();
	QString currentDate = dt.toString("yyyy-MM-dd-hh-mm-ss");  

	QString filterE = "Csv File(*.csv)";
	QString filePathE = "./trajectory/" + currentDate + "-E" + ".csv";
	QString fileNameE = QFileDialog::getSaveFileName(this, tr("Load..."), filePathE, filterE);

	if (fileNameE.isEmpty())
		fileNameE = filePathE;

	traj->saveE(fileNameE);
	QFileInfo fileInfo(fileNameE);
	traj->name = fileInfo.baseName();

	QString filterT = "Csv File(*.csv)";
	QString filePathT = "./trajectory/" + currentDate + "-T" + ".csv";
	QString fileNameT = QFileDialog::getSaveFileName(this, tr("Load..."), filePathT, filterT);

	if (fileNameT.isEmpty())
		fileNameT = filePathT;

	traj->saveT(fileNameT);

	Macro::s_record = false;
	Macro::s_timeStep = 0.0;
}

void FrameWork::loadPath()
{
	QString filter = "Csv File(*.csv)";
	QString filePath = "./trajectory/";
	QString fileName = QFileDialog::getOpenFileName(this, tr("Load..."), filePath, filter);

	if (!fileName.isEmpty())
	{
		traj->load(fileName);
		QFileInfo fileInfo(fileName);
		traj->name = fileInfo.baseName();
	}
}

void FrameWork::pausePath()
{
	traj->pause();
}

void FrameWork::resetData()
{
	pw->reset();
	ccd->reset();
	model->loadModel("data/ball.xml");
	rd->reset();
}

void FrameWork::resetParam()
{
	Macro::reset();

	replayIterEditor->setValidator(new QIntValidator(1, 100, this));
	muSlider1->setValue((int)(Macro::s_mu1 / 20 * 1000));
	muSlider2->setValue((int)(Macro::s_mu2 / 10 * 500));
	dMass m;
	dBodyGetMass(pw->magicBoxBody, &m);
	massSpinBox->setValue(m.mass);
}

void FrameWork::resetAll()
{
	resetData();
	resetParam();
	traj->reset();
}

void FrameWork::replay()
{
	resetData();

	dMass m;
	dBodyGetMass(pw->magicBoxBody, &m);
	dMassAdjust(&m, massSpinBox->value());
	dBodySetMass(pw->magicBoxBody, &m);

	Macro::s_replay = true;
	rd->setTrajectory(traj);
}

void FrameWork::changeReplayIters(const QString & text)
{
	Macro::s_replayIters = text.toInt();
}

void FrameWork::changeValueMuSlider1(double val)
{
	Macro::s_mu1 = val;
	muSlider1->setValue((int)(val / 20 * 1000));
}

void FrameWork::changeValueMuSlider2(double val)
{
	Macro::s_mu2 = val;
	muSlider2->setValue((int)(val / 10 * 500));
}

void FrameWork::changeValueMuSpinBox1(int val)
{
	Macro::s_mu1 = (double)val / 1000 * 20;
	muSpinBox1->setValue(Macro::s_mu1);
}

void FrameWork::changeValueMuSpinBox2(int val)
{
	Macro::s_mu2 = (double)val / 500 * 10;
	muSpinBox2->setValue(Macro::s_mu2);
}

void FrameWork::changeValueMassSpinBox(double val)
{
	dMass m;
	dBodyGetMass(pw->magicBoxBody, &m);
	dMassAdjust(&m, val);
	massSpinBox->setValue(val);
}